﻿//#include"winsock2.h"
#include <vector>
#include <list>
#include<cmath>
#include<iostream>
#include<time.h>
#include<cstdlib>
#include<thread>
#include<math.h>
#include<algorithm>
#include<string>
#include<stdlib.h>
#include<mutex>
#include<boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
//这是linux下的 暂时先这样写
//#include <unistd.h>
namespace bg = boost::geometry;
namespace bgm = bg::model;
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

typedef boost::tuple<double, double> point;
typedef bgm::d2::point_xy<double> point_xy;

typedef bgm::multi_point<point_xy> points;

typedef bgm::polygon<point_xy> hull_point;
typedef bgm::polygon<point_xy> polygon;

typedef unsigned char      uint8_t;

void usleep(unsigned long usec);  //这是liux中的函数

using namespace std;

#define PI 3.1415926   //圆周率
const double EARTH_RADIUS = 6378.137;   //地球半径
//先定义一个起始坐标点
#define START_LON 125.1631713
#define START_LAT 43.8324610
//车辆和路侧检测到的障碍物数据
//进行冗余检测时需要用到的数据1
struct map_world
{
    float lon;
    float lat;
};
struct coordinate
{
    float x;
    float y;
};
struct bbox
{
    //识别是车还是路侧
    //char类型  值是ASCII 码翻译后，0x52='R'，0x56='V'
   // char message_source;
    uint8_t id_buf;
    //障碍物类别
    long classification;
    //置信度
    float score;
    //将四个顶点坐标存到这个数组中
    coordinate apex_coordinate[4];
    //存中心点坐标
    coordinate center_coordinate;
    //长宽高
    float height;
    float length;
    float width;
    //障碍物航向角
    float heading;
    //障碍物速度
    float speed_object;
    //障碍物横纵向速度
    float x_speed;
    float y_speed;
    //障碍物经纬度
    float lon;
    float lat;
};
static double rad(double d)
{
    return d * PI / 180;
}
//根据经纬度求两点间距离
float cacu_gps_dis(map_world H_map, map_world R_map)
{
    double radLat1 = rad(H_map.lat);
    double radLat2 = rad(R_map.lat);
    double a = radLat1 - radLat2;
    double b = rad(H_map.lon) - rad(R_map.lon);
    double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(b / 2), 2)));
    s = s * EARTH_RADIUS;
    s = s * 1000;
    return s;
}
//根据经纬度求两点之间夹角
float get_angle(map_world map_a, map_world map_b)
{
    double x = map_a.lat - map_b.lat;
    double y = map_a.lon - map_b.lon;
    int angle = -1;
    if (y == 0 && x > 0) angle = 0;
    if (y == 0 && x < 0) angle = 180;
    if (x == 0 && y > 0) angle = 90;
    if (x == 0 && y < 0) angle = 270;
    if (angle == -1)
    {
        map_world map_c, map_d;
        map_c.lon = map_b.lon; map_c.lat = map_a.lat;
        map_d.lon = map_a.lon; map_d.lat = map_b.lat;
        double dislat = cacu_gps_dis(map_c, map_b);
        double dislng = cacu_gps_dis(map_d, map_b);
        if (x > 0 && y > 0) angle = atan2(dislng, dislat) / PI * 180;
        if (x < 0 && y > 0) angle = atan2(dislat, dislng) / PI * 180 + 90;
        if (x < 0 && y < 0) angle = atan2(dislng, dislat) / PI * 180 + 180;
        if (x > 0 && y < 0) angle = atan2(dislat, dislng) / PI * 180 + 270;
    }
    return angle;
}
/**
 * @brief 将point数组转为内部使用的格式
 *
 * @param p point array
 * @return points
 */
points convert(vector<point> p)
{
    points res;
    for (auto i = p.begin(); i != p.end(); i++)
    {
        res.push_back(point_xy(i->head, i->tail.head));
    }
    return res;
}

/**
 * @brief Get the Hull Points object, 求出顺时针的凸包
 *
 * @param p 任意顺序的四个点
 * @return bgm::polygon<point>
 */
hull_point getHullPoints(points& p)
{
    bgm::polygon<point_xy> hull;
    bg::convex_hull(p, hull);
    return hull;
}

/**
 * @brief Get the Intersection Area object
 *
 * @param rect1 第一个长方形
 * @param rect2 第二个长方形
 * @return double
 */
double getIntersectionArea(polygon rect1, polygon rect2)
{
    std::deque<polygon> output;
    bg::intersection(rect1, rect2, output);
    auto polygon_output = output.begin();
    if (output.size() == 0)
    {
        return 0;
    }
    else
    {
        return bg::area(*polygon_output);
    }
}

/**
 * @brief Get the Union object
 *
 * @param rect1
 * @param rect2
 * @return double
 */
double getUnion(polygon rect1, polygon rect2) {
    return bg::area(rect1) + bg::area(rect2);
}

/**
 * @brief
 *
 * @param rect1 {{-1, 0}, {1, 1}, {1, 0}, {-1, 1}}
 * @param rect2 {{0, 0}, {1, 1}, {0, 2}, {-1, 1}}
 * @return double
 */
double get_IOU(vector<point> rect1, vector<point> rect2) {
    //转换格式
    points rect1_point = convert(rect1);
    points rect2_point = convert(rect2);
    //将两个矩形变成凸包
    auto hull1 = getHullPoints(rect1_point);
    auto hull2 = getHullPoints(rect2_point);
    //求交集
    auto area_intersection = getIntersectionArea(hull1, hull2);
    //求并集
    auto area_union = getUnion(hull1, hull2) - area_intersection;

    return area_union / area_intersection;
}
//计算路侧的中心点坐标
void cacu_center_xy(vector<bbox>& b)
{
    map_world start_lon_lat, end_lon_lat;
    start_lon_lat.lon = START_LON; start_lon_lat.lat = START_LAT;
    float distance, azimuth;
    for (int i = 0; i < b.size(); i++)
    {
        end_lon_lat.lon = b[i].lon; end_lon_lat.lat = b[i].lat;
        distance = cacu_gps_dis(start_lon_lat, end_lon_lat);
        azimuth = rad(get_angle(start_lon_lat, end_lon_lat));
        b[i].center_coordinate.x = cos(azimuth) * distance;
        b[i].center_coordinate.y = sin(azimuth) * distance;
    }
}
//计算障碍物信息计算坐标
void cacu_pos_apex(bbox& box1)
{
    //路端和车端的heading指的是路侧检测到的障碍物的heading
    //先将障碍物和世界坐标系方向一致
    float theta = box1.heading;
    float rotation_matrix[2][2];
    rotation_matrix[0][0] = cos(theta);
    rotation_matrix[0][1] = -sin(theta);
    rotation_matrix[1][0] = sin(theta);
    rotation_matrix[1][1] = cos(theta);
    box1.apex_coordinate[0].x = -rotation_matrix[0][0] * box1.width / 2 + rotation_matrix[0][1] * box1.length / 2;
    box1.apex_coordinate[0].y = -rotation_matrix[1][0] * box1.width / 2 + rotation_matrix[1][1] * box1.length / 2;
    box1.apex_coordinate[1].x = rotation_matrix[0][0] * box1.width / 2 + rotation_matrix[0][1] * box1.length / 2;
    box1.apex_coordinate[1].y = rotation_matrix[1][0] * box1.width / 2 + rotation_matrix[1][1] * box1.length / 2;
    box1.apex_coordinate[3].x = -rotation_matrix[0][0] * box1.width / 2 - rotation_matrix[0][1] * box1.length / 2;
    box1.apex_coordinate[3].y = -rotation_matrix[1][0] * box1.width / 2 - rotation_matrix[1][1] * box1.length / 2;
    box1.apex_coordinate[2].x = rotation_matrix[0][0] * box1.width / 2 - rotation_matrix[0][1] * box1.length / 2;
    box1.apex_coordinate[2].y = rotation_matrix[1][0] * box1.width / 2 - rotation_matrix[1][1] * box1.length / 2;
    //进行平移
    //这里的经纬度也是指的路侧检测到的障碍物的经纬度
    //以世界坐标系为起点进行平移
    //平移变换
    for (int i = 0; i < 4; i++)
    {
        box1.apex_coordinate[i].x = box1.apex_coordinate[i].x + box1.center_coordinate.x;
        box1.apex_coordinate[i].y = box1.apex_coordinate[i].y + box1.center_coordinate.y;
    }
}
bool comp(const bbox& box1, const bbox& box2)
{
    return box1.score > box2.score;
}
//根据置信度进行排序,决定比较顺序
void NMS(vector<bbox>& bb)
{
    //根据置信度进行排序
    sort(bb.begin(), bb.end(), comp);
    vector<bbox>::iterator ib = bb.begin();
    while (ib != bb.end())
    {
        vector<bbox>::iterator it = ib;
        it++;
        while (it != bb.end())
        {
            //对每一个需要进行求iou的矩形框求出坐标
            //已知障碍物的经纬度就直接求
            cacu_pos_apex(*it); 
            cacu_pos_apex(*ib);
            //存入point中
            vector<point> comp_rect1, comp_rect2;
            for (size_t i = 0; i < 4; i++)
            {
                comp_rect1.push_back({ (*ib).apex_coordinate[i].x,(*ib).apex_coordinate[i].y });
                comp_rect2.push_back({ (*it).apex_coordinate[i].x,(*it).apex_coordinate[i].y });
            }
            comp_rect1.push_back({ (*ib).apex_coordinate[0].x,(*ib).apex_coordinate[0].y });
            comp_rect2.push_back({ (*it).apex_coordinate[0].x,(*it).apex_coordinate[0].y });
            //当iou大于阈值时，将删除此box，进行下一个比较
            if (get_IOU(comp_rect1, comp_rect2) > 0.6)
            {
                it = bb.erase(it);
            }
            else {
                it++;
            }
        }
        ib++;
    }
}
//这里假设car_obtacle_list是车端障碍物列表
//road_obtacle_list是路侧障碍物列表
//函数类型和参数类型都改成rsm格式
//此时是一个进程，用void类型
RoadsideSafetyMessage_t Merge(RoadsideSafetyMessage_t& road_msg, RoadsideSafetyMessage_t& vehi_msg)
{
    //因为不做区分方块，因此直接放到一个vector中做nms
    vector<bbox> obtacle_list;
    //将主车和路侧的车端信息赋值给定义的结构体
    //将车的数据赋值到car_obtacle_list
    bbox temporary_box;
    for (int i = 0; i < vehi_msg->participants.list.count; i++)
    {
        temporary_box.id_buf = 'V';
        temporary_box.classification = vehi_msg->participants.list.array[i]->ptcType;
        temporary_box.score = atof(vehi_msg->participants.list.array[i]->id.buf) * 0.01;
        temporary_box.height = vehi_msg->participants.list.array[i]->size.height;
        temporary_box.width = vehi_msg->participants.list.array[i]->size->width;
        temporary_box.length = vehi_msg->participants.list.array[i]->size->length;
        temporary_box.lon = vehi_msg->participants.list.array[i]->pos->offsetLL->position_LL1->lon + vehi_msg->refPos->Long;
        temporary_box.lat = vehi_msg->participants.list.array[i]->pos->offsetLL->position_LL1->lat + vehi_msg->refPos->Lat;
        temporary_box.speed_object = vehi_msg->participants.list.array[i]->speed;
        temporary_box.heading = rad(vehi_msg->participants.list.array[i]->heading);//车端提供的也是障碍物的航向角
        temporary_box.x_speed = cos(temporary_box.heading) * temporary_box.speed_object;
        temporary_box.y_speed = sin(temporary_box.heading) * temporary_box.speed_object;
        obtacle_list.push_back(temporary_box);
    }
    //将路侧设备的信息road_msg赋值到road_obtacle_list
    for (int i = 0; road_msg->participants.list.count; i++)
    {
        //在进行消息队列传入时，已经识别出消息是来源于车端还是路端了，因此此时直接命明就好
        temporary_box.id_buf = 'R';
        temporary_box.classification = road_msg->participants.list.array[i]->ptcType;
        temporary_box.score = atof(road_msg->participants.list.array[i]->id.buf)*0.01;
        temporary_box.height = road_msg->participants.list.array[i]->size.height;
        temporary_box.width = road_msg->participants.list.array[i]->size->width;
        temporary_box.length = road_msg->participants.list.array[i]->size->length;
        temporary_box.lon = road_msg->participants.list.array[i]->pos->offsetLL->position_LL1->lon + road_msg->refPos->Long;
        temporary_box.lat = road_msg->participants.list.array[i]->pos->offsetLL->position_LL1->lat + road_msg->refPos->Lat;
        temporary_box.speed_object = road_msg->participants.list.array[i]->speed;
        temporary_box.heading = rad(road_msg->participants.list.array[i]->heading);//路端的障碍物航向角
        temporary_box.x_speed = temporary_box.speed_object*cos(temporary_box.heading);
        temporary_box.y_speed = temporary_box.speed_object *sin(temporary_box.heading);
        obtacle_list.push_back(temporary_box);
    }
    //障碍物中心点坐标
    cacu_center_xy(obtacle_list);
    NMS(obtacle_list);
    usleep(20000);//匹配就调用，但每20ms执行一次计算
}